/*   Copyright (c) 2008, Brian Griffiths
 *   All rights reserved.
 *
 *   Redistribution and use in source and binary forms, with or without
 *   modification, are permitted provided that the following conditions are met:
 *       * Redistributions of source code must retain the above copyright
 *         notice, this list of conditions and the following disclaimer.
 *       * Redistributions in binary form must reproduce the above copyright
 *         notice, this list of conditions and the following disclaimer in the
 *         documentation and/or other materials provided with the distribution.
 *       * Neither the name of Brian Griffiths nor the
 *         names of this packages contributors may be used to endorse or promote products
 *         derived from this software without specific prior written permission.
 *
 *   THIS SOFTWARE IS PROVIDED BY Brian Griffiths ``AS IS'' AND ANY
 *   EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 *   WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL Brian Griffiths BE LIABLE FOR ANY
 *   DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 *   (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 *   ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 *   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/* Warning: This software will produce errors if the input
 * data crosses the meridian/date-line/equator */

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>

#include <EEPROM.h>
#include <avr/sleep.h>

#include "breadcrumbs.h"

/************************************************************/
/* Global decelerations                                     */

char    buf[10];
char    fnd         = false;
char    decode_mode = false;
char    first_fix   = true;
char    degNorth    = true;
char    degEast     = false;
char    *pEnd;
int     p;
int     fieldId     = 0;
int     byte_cnt    = 0;
float   lat, lon;
gpsPos  current, previous;

int ledPin = 13;                  /* LED test pin */
int selPin = 5;                   /* Mode select pin */
int rxPin = 0;                    /* RX PIN */
int txPin = 1;                    /* TX PIN */

void setup()
{
    pinMode(ledPin, OUTPUT);
    digitalWrite(selPin,HIGH);
    pinMode(selPin, INPUT);
    pinMode(rxPin, INPUT);
    pinMode(txPin, OUTPUT);

    /* link 'on' to record */
    if(digitalRead(selPin) == 0) decode_mode = false;
    else decode_mode = true;

    Serial.begin(4800);

    memset(buf, 0, sizeof(buf));
}


void loop(void)
{
	if(decode_mode)
	{
		gpsDecodeEeprom();
		set_sleep_mode(SLEEP_MODE_PWR_DOWN);
		sleep_enable();
		sleep_mode();
	}

    if (!fnd)
        p = 2;
    else
        p = 10;

    for (int i = 0; i < p; i++)
    {
        buf[i] = buf[i + 1];
    }

    buf[p] = -1;
    while(buf[p] == -1)
    {
        SER_READ(buf[p]);
        if (buf[p] == -1) {           // See if the port is empty yet
            delay(100);
        }
    }

    if (buf[p] == (char) 10) /* ASCII 10 = line feed */
    {
        fnd = false;
        fieldId = 0;
        digitalWrite(ledPin, LOW);
    }

    if (buf[0] == 'G' && buf[1] == 'G' && buf[2] == 'A')
        fnd = true;

    /* Use the comma as a separator to identify the entry number and
     * examine the data. */
    if (fnd && buf[p] == ',')
    {
        fieldId++;
        buf[p] = '0';
        switch (fieldId)
        {
            case (7): /* Fix Quality - 0=invalid 1=GPS fix*/
                fnd = false;
                if(buf[p-1] >= '1')
                {
                    gpsEncodePosition(lat, lon);
                }
                break;
            case (3): /* Latitude */
                lat = (float)strtod(buf,&pEnd);
                break;
            case (5): /* Longitude */
                lon = (float)strtod(buf,&pEnd);
                /* flush data waiting to be sent to the EEPROM */
                gpsWriteEeprom((void *)0,0);
                digitalWrite(ledPin, HIGH);
	            break;
            case (4): /* N/S */
                degNorth = (buf[p-1] == 'N' ? true : false);
                break;
            case (6): /* W/E */
                degEast = (buf[p-1] == 'E' ? true : false);
                break;
            default:
                break;
        }
    }
}

/************************************************************/
/* Encode the current position to EEPROM                    */

void gpsEncodePosition(float lat, float lon)
{
    float d;
    float latd, lond;
    unsigned int dir;
    static gpsPos estimated;

    current.lat = gpsDm2Dec(lat);
    current.lon = gpsDm2Dec(lon);

    /* If this is the first fix write the full position to EEPROM */
    if(first_fix)
    {
    	unsigned long lat_scaled = (unsigned long)(current.lat * POS_MULT);
        unsigned long lon_scaled = (unsigned long)(current.lon * POS_MULT);

    	first_fix = false;

    	if(!degNorth) lat_scaled |= (unsigned long)1 << (FULL_RES-1);
    	if(!degEast)  lon_scaled |= (unsigned long)1 << (FULL_RES-1);

    	/* blank the EEPROM */
        for(int i = 0;i< EEPROM_SIZE;i++) EEPROM.write(i, 0);

    	gpsWriteEeprom(&lat_scaled, FULL_RES);
        gpsWriteEeprom(&lon_scaled, FULL_RES);

        memcpy(&previous, &current, sizeof(gpsPos));
        memcpy(&estimated, &current, sizeof(gpsPos));
    }
    else do
    {
        latd = fabs(current.lat - previous.lat);
        lond = fabs(current.lon - previous.lon);

        /* calculate the distance from the previous position */
        d = sqrt((latd*latd) + (lond*lond));

        /* calculate the heading of the new position */
        gpsCalcHeading(&previous, &current);

        /* if the new position is greater than on minimum record this heading */
        if(d > (float)STEP_SIZE/POS_MULT)
        {
            dir = (unsigned int)current.direction / DIR_SCALE;

            gpsWriteEeprom(&dir, DIR_RES);

            /* Recalculate (using the new heading) what will become the previous position */
            gpsAddDir(&estimated.lat, &estimated.lon, dir * DIR_SCALE);

            memcpy(&previous, &estimated, sizeof(gpsPos));
        }
    } while(d > (unsigned long)(STEP_SIZE));
}

/************************************************************/
/* degrees/deciminutes (NMEA format) to decimal degrees     */

float gpsDm2Dec(float dms)
{
    double deg, min;

    modf(dms / 100.0, &deg);
    min = dms - (deg * 100);

    return (float)deg+((float)min/60.0);
}

/************************************************************/
/* Calculate the heading (from north) between two positions */

void gpsCalcHeading(gpsPos *prev, gpsPos *cur)
{
    cur->norm_dir = RAD2DEG(atan(fabs(prev->lat-cur->lat)/
                                 fabs(prev->lon-cur->lon)));

    if (cur->lat >= prev->lat && prev->lon >= cur->lon)
    {
        cur->direction = 90.0 - cur->norm_dir;
        cur->quad = 0;
    }
    else if (prev->lat >= cur->lat && prev->lon >= cur->lon)
    {
        cur->direction = 90.0 + cur->norm_dir;
        cur->quad = 1;
    }
    else if (prev->lat > cur->lat && cur->lon > prev->lon)
    {
        cur->direction = 270.0 - cur->norm_dir;
        cur->quad = 2;
    }
    else
    {
        cur->direction = 270.0 + cur->norm_dir;
        cur->quad = 3;
    }
}
/************************************************************/
/* Translate an already recorded EEPROM session into KML    */

void gpsDecodeEeprom(void)
{
	unsigned long lat = 0,lon = 0;
	unsigned int dir = 0;
	float latf, lonf;

	/* Read the first fix latitude */
	gpsReadEeprom(&lat, FULL_RES);
	/* If the sign bit is raised create a flag we can use
	 * to make the correct coordinate calculations */
    degNorth = (lat & ((unsigned long)1 << (FULL_RES-1))) ? false : true;
    lat &= ~((unsigned long)1 << (FULL_RES-1));
	latf = (float)lat / POS_MULT;

	/* Repeat for longitude */
	gpsReadEeprom(&lon, FULL_RES);
	degEast  = (lon & ((unsigned long)1 << (FULL_RES-1))) ? false : true;
    lon &= ~((unsigned long)1 << (FULL_RES-1));
    lonf = (float)lon / POS_MULT;

	/* Create a place mark entry with the first fix */
    SER_WRITE(KML_STRING1);
    if(degEast) {
        SER_WRITE_D(lonf);
    } else {
        SER_WRITE_D((0.0-lonf));
    }
    SER_WRITE(",");
    if(degNorth) {
        SER_WRITE_D(latf);
    } else {
        SER_WRITE_D((0.0-latf));
    }
    SER_WRITE("\n");
	SER_WRITE(KML_STRING2);

	for(;;)
	{
        /* Read a heading from EEPROM */
	    gpsReadEeprom(&dir, DIR_RES);

        if(byte_cnt > EEPROM_SIZE) break;

        if(dir == 0 && gpsCheckEnd()) break;

        /* Add it to our current position */
        gpsAddDir(&latf, &lonf, dir * DIR_SCALE);

        /* Add to the KML Line String */
        if(degEast) {
            SER_WRITE_D(lonf);
        } else {
            SER_WRITE_D((0.0-lonf));
        }
        SER_WRITE(",");
        if(degNorth) {
            SER_WRITE_D(latf);
        } else {
            SER_WRITE_D((0.0-latf));
        }

        SER_WRITE(" <!--");
        SER_WRITE_A(dir);

        SER_WRITE("-->\n");
	}
	SER_WRITE(KML_STRING3);
}

/************************************************************/
/* Check to see if the remaining EEPROM is empty            */

char gpsCheckEnd(void)
{
    for(int i=byte_cnt;i<EEPROM_SIZE;i++)
    {
        if(EE_READ(i) != 0) return false;
    }
    return true;
}

/************************************************************/
/* Add our step size to a position using our heading        */

void gpsAddDir(float *lat, float *lon, unsigned int dir)
{
    float opp, adj;
    int quad;

    /* Work out which quadrant this heading is in */
    if(dir <= 90)
    {
        quad = 0;
        dir = 90 - dir;
    }
    else if (dir > 90 && dir < 180)
    {
        quad = 1;
        dir = dir - 90;
    }
    else if (dir >= 180 && dir <270)
    {
        quad = 2;
        dir = 270 - dir;
    }
    else
    {
        quad = 3;
        dir = dir - 270;
    }

    /* the trigonometric relationship between the heading and our step size */
    opp = sin(DEG2RAD((float)dir)) * ((float)(STEP_SIZE) / POS_MULT);
    adj = cos(DEG2RAD((float)dir)) * ((float)(STEP_SIZE) / POS_MULT);

    /* based on the quadrant of our heading re-calculate the new position */
    switch(quad)
    {
        case 0:
            *lon -= adj;
            *lat += opp;
            break;
        case 1:
            *lon -= adj;
            *lat -= opp;
            break;
        case 2:
            *lon += adj;
            *lat -= opp;
            break;
        default:
            *lon += adj;
            *lat += opp;
            break;
    }
}

/************************************************************/
/* Read n bits from the EEPROM                              */

void gpsReadEeprom(void *data, int nBits)
{
    int outp = 0;
    static unsigned char dat;
    static int inp = 0;
    int bc=0;
    static char first_pass = true;

    if(first_pass)
    {
        dat = EE_READ(byte_cnt);
        byte_cnt++;
        first_pass = false;
    }

    for(int i=0;i<nBits;i++)
    {
        if(dat & (1 << inp))
        {
            SETBIT(*((unsigned char *)data + bc),outp);
        }
        else
        {
            UNSETBIT(*((unsigned char *)data + bc),outp);
        }

        outp++;
        inp++;

        if(inp == 8)
        {
            inp = 0;
            dat = EE_READ(byte_cnt);
            byte_cnt++;
        }

        if(outp == 8)
        {
            outp = 0;
            bc++;
        }
    }
}

/************************************************************/
/* Write n bits from the EEPROM                             */


void gpsWriteEeprom(void *data, int nBits)
{
    static int outp = 0;
    static unsigned char dat;
    int inp = 0;
    int bc=0;

    /* nBits == 0 indicates we must flush */
    if(nBits == 0)
    {
        int bits_left = 7-outp;

        if(bits_left == 7) return;

        for(int i=0;i<bits_left;i++)
        {
            UNSETBIT(dat,(outp+i));
        }

        EE_WRITE(dat, byte_cnt);
    }
    else
    {
        for(int i=0;i<nBits;i++)
        {
            if(*((unsigned char *)data + bc) & (1 << inp))
            {
                SETBIT(dat,outp);
            }
            else
            {
                UNSETBIT(dat,outp);
            }

            outp++;
            inp++;

            if(inp == 8)
            {
                inp = 0;
                bc++;
            }

            if(outp == 8)
            {
                outp = 0;
                EE_WRITE(dat, byte_cnt);
                byte_cnt++;
                if(byte_cnt == EEPROM_SIZE) {
                    set_sleep_mode(SLEEP_MODE_PWR_DOWN);
                    sleep_enable();
                    sleep_mode();
                }
            }
       }
    }
}
